/*
 * pidcontroller.h
 *
 *  Created on: 18-12-6
 *      Author: hailiang
 */

#ifndef _PIDCONTROLLER_H
#define _PIDCONTROLLER_H

/**
 * 位置型PID控制算法
 */
class PidController {
protected:
    double kp_;
    double ki_;
    double kd_;

    double set_value_;
    double integral_value_;
    double last_error_;

    double output_value_;
    double max_value_;
    double min_value_;

public:
    PidController(double kp, double ki, double kd, double set_value, double max_value, double min_value) {
        kp_        = kp;
        ki_        = ki;
        kd_        = kd;
        set_value_ = set_value;
        max_value_ = max_value;
        min_value_ = min_value;

        last_error_     = 0;
        integral_value_ = 0;
        output_value_   = min_value_;
    }

    double pidOperation(double feedback) {
        double err;

        err = set_value_ - feedback;
        integral_value_ += err;
        output_value_ = kp_ * err + ki_ * integral_value_ + kd_ * (err - last_error_);

        if (output_value_ > max_value_) {
            output_value_ = max_value_;
        } else if (output_value_ < min_value_) {
            output_value_ = min_value_;
        }
        last_error_ = err;

        return output_value_;
    }

    void   pidSetParam(double kp, double ki, double kd) {
        kp_ = kp;
        ki_ = ki;
        kd_ = kd;
    }

    void   pidSetTarget(double set_value) {
        set_value_ = set_value;
    }

    double pidGetState() {
        return output_value_;
    }

    void   pidClearValue() {
        last_error_     = 0;
        integral_value_ = 0;
    }

    double pidGetkp() {
        return kp_;
    }
};

#endif /* _PIDCONTROLLER_H */
